#include "MyISP.h" 


void DPC_Weighted(ImageRaw& raw, uint16_t thres, uint16_t clip)
{
    ImageRaw* raw_pad = new ImageRaw(raw);
    raw_pad->padding(2, PADDING_MODE_REFLECT);

    for (int y = 0; y < raw_pad->getHeight() - 4; y++)
    {
        for (int x = 0; x < raw_pad->getWidth() - 4; x++)
        {
            uint16_t p0, p1, p2, p3, p4, p5, p6, p7, p8;
            uint16_t neighbor_values[8];
            double weights[8];
            double sum_weights = 0.0;
            double weighted_sum = 0.0;

            p0 = raw_pad->at(y + 2, x + 2);
            p1 = raw_pad->at(y, x);
            p2 = raw_pad->at(y, x + 2);
            p3 = raw_pad->at(y, x + 4);
            p4 = raw_pad->at(y + 2, x);
            p5 = raw_pad->at(y + 2, x + 4);
            p6 = raw_pad->at(y + 4, x);
            p7 = raw_pad->at(y + 4, x + 2);
            p8 = raw_pad->at(y + 4, x + 4);

            neighbor_values[0] = p1;
            neighbor_values[1] = p2;
            neighbor_values[2] = p3;
            neighbor_values[3] = p4;
            neighbor_values[4] = p5;
            neighbor_values[5] = p6;
            neighbor_values[6] = p7;
            neighbor_values[7] = p8;

            if ((ABS(p1 - p0) > thres) && (ABS(p2 - p0) > thres) && (ABS(p3 - p0) > thres) && (ABS(p4 - p0) > thres) &&
                (ABS(p5 - p0) > thres) && (ABS(p6 - p0) > thres) && (ABS(p7 - p0) > thres) && (ABS(p8 - p0) > thres))
            {
                for (int i = 0; i < 8; i++)
                {
                    double dist = ABS(neighbor_values[i] - p0);
                    if (dist > 0)
                    {
                        weights[i] = 1.0 / dist;
                    }
                    else
                    {
                        weights[i] = 1.0;
                    }
                    sum_weights += weights[i];
                    weighted_sum += neighbor_values[i] * weights[i];
                }
                p0 = static_cast<uint16_t>(weighted_sum / sum_weights);
                raw.at(y, x) = p0;
            }
        }
    }
    raw.clip(0, clip);
    delete raw_pad;
}